--[[ Functions to call in mission editor:
setupRadioMenu()

joinedDist = 200
joinedHeight = 50
setJoinedZone(joinedDist, joinedHeight)
setMaxRejoinTime(10)
setWaveOffDistance(1000)

maxMnDist = 150
maxStDevDist =50
maxMnHeight = 20
maxStdDevHeight =20
setRequirements(maxMnDist,maxStDevDist,maxMnHeight,maxStdDevHeight)

setEvaluationTimes(60,360)
setMaxBreaks(3)

setLeader("Leader",1)
setWingman("Wingman",1)

setSuccesFlag(1)
setFailFlag(2)
]]

local flagSucces = 1
local flagFail = 2
local statStartTime = 30
local statEndTime = 40
local maxRejoinTime = 10

local joinedDist = math.huge
local joinedHeight = math.huge
local timeOfLastBreak = nil
local waveOffDistance = math.huge
local numBreaks = 0
local maxBreaks = 3
local numJoins = 0

local maxMnDist = math.huge
local maxStDevDist = math.huge
local maxMnHeight = math.huge
local maxStdDevHeight = math.huge

local leader = nil
local wingman = nil
local msg = ""

local distData = {}
local heightData = {}
local iData = 1
local maxData = 1000
local statsRunning = false

local sndDING = "DING.wav"
local sndOK = "OK.wav"
local sndNOK = "NOK.wav"

local curDist = math.huge
local curRadVel = math.huge
local curHeight = math.huge

local showPos = true
local showStats = true

--! The flag @p fl is set to true on a mission succes.
function setSuccesFlag(fl)
	flagSucces = fl
end

--! The flag @p fl is set to true on a mission fail.
function setFailFlag(fl)
	flagFail = fl
end

--! Set the maximum number of allowed formation breaks. A break occurs when the wingman leaves the joined zone. @see setJoinedZone.
function setMaxBreaks(mb)
	maxBreaks = mb
end

--[[!
@brief Sets the mission requirements.
@p mmd The maximum allowed mean distance in meters.
@p msd The maximum allowed standard deviation of the distance in meters.
@p mmh The maximum allowed mean height in meters.
@p msh The maximum allowed standard deviation of the height in meters.
]]
function setRequirements(mmd, msd, mmh, msh)
	maxMnDist = mmd
	maxStDevDist = msd
	maxMnHeight = mmh
	maxStdDevHeight = msh
end

--[[!
@brief Set the flight lead unit.
@p name The group name of the wingman.
@p number The unit number of the wingman.
]]
function setLeader(name, number)
	leader = Group.getByName(name):getUnit(number)
end

--[[!
@brief Set the wingman unit.
@p name The group name of the wingman.
@p number The unit number of the wingman.
]]
function setWingman(name, number)
	wingman = Group.getByName(name):getUnit(number)
end

--[[! Set a "pill-box" around the leader with radius @p d (meters) and height @p h (meters). If the wingman is less than
@p d meters from the leader and the height difference is less than @p h meters, then the wingman is considered to be joined.
]]
function setJoinedZone(d, h)
	joinedDist = d
	joinedHeight = h
end

--! Set the maximum allowed instantaneous distance between wingman and leader to @p d meters.
function setWaveOffDistance(d)
	waveOffDistance = d
end

--! Set the time, in seconds, at which the evaluation should start @p start and stop @p stop.
function setEvaluationTimes(start, stop)
	statStartTime = start
	statEndTime = stop
end

--! Sets the maximum allowed time @p t (seconds) between a break and a re-join.
function setMaxRejoinTime(t)
	maxRejoinTime = t
end

--! Sets the message @p m, which will be displayed alongside with the current position and statistics.
function setMessage(m)
	msg = m
end

--[[!
Computes the sample mean the elements of array @p a.
@returns m mean
]]
function mean(a)
    local m = 0
    for i = 1,#a do
        m = m + a[i]
    end
    m = m / #a
    return m
end

--[[!
Computes the sample standard deviation of the elements of array @p a.
@returns s the standard deviation
]]
function stdev(a)
    local mn = mean(a)
    local s = 0

    for i = 1,#a do
        local d = a[i] - mn
        s = s + d*d
    end
    s = math.sqrt(s/#a)
    return s
end

--[[!
@brief Computes the distance between the leader and the wingman.

@p range is the slant range between leader and wingman.
@p disth is the distance measured in the horizontal plane.
@p y is the height difference.

@returns range, disth, y
]]
function computeDistance()
	local pl = leader:getPoint()
	local pw = wingman:getPoint()

	local x = pl.x - pw.x
	local y = pl.y - pw.y
	local z = pl.z - pw.z

	local disth2 = x*x + z*z
	local disth = math.sqrt(disth2)
	local range = math.sqrt(disth2 + y*y)
	
	return range, disth, y
end

--[[!
@brief Computes the relative and radial velocity between the leader and the wingman. 

The relative velocity is simply
the difference between the velocity of the leader and the velocity of the wingman. The radial velocity is the relative 
velocity measured along the line between the wingman and the leader. It is positive if the wingman is approaching the leader
from behind.

@returns relVel, radVel.
]]
function computeRelativeVelocity()
	-- Compute the relative velocity:
	local vl = leader:getVelocity()
	local vw = wingman:getVelocity()

	local vx = vl.x - vw.x
	local vy = vl.y - vw.y
	local vz = vl.z - vw.z

	local relVel = math.sqrt(vx*vx + vy*vy + vz*vz)

	-- Compute the radial velocity:
	local pl = leader:getPoint()
	local pw = wingman:getPoint()

	local x = pl.x - pw.x
	local y = pl.y - pw.y
	local z = pl.z - pw.z

	local radVel = (x*vx + y*vy + z*vz)/math.sqrt(x*x+y*y+z*z)
	
	return relVel, radVel
end

--[[!
@brief Compute position and velocity and store values in curDist and curHeight.

For scoring the current distance and height are added to the distData and heightData arrays.
]]
function updatePosition()
	local range, disth, height = computeDistance()
	curDist = disth
	curHeight = height

	local relVel, radVel = computeRelativeVelocity()
	curRadVel = radVel

	statStr = ""
	if statsRunning then
		distData[iData]=disth
		heightData[iData]=height
		
		iData = iData + 1
		if iData > maxData then
			iData = 1
		end
	end
end

--[[!
@brief Updates the isJoined variable, which indicates if the wingman is close enough to the leader. 
]]
function checkJoined()
	local timeSinceStart = timer.getAbsTime() - timer.getTime0()
	isJoinedNow = curDist < joinedDist and math.abs(curHeight) < joinedHeight
	if isJoined and not isJoinedNow then
		setMessage("You broke formation!")
		trigger.action.outSound(sndNOK)
		timeOfLastBreak = timeSinceStart
		numBreaks = numBreaks + 1
	elseif not isJoined then
		if isJoinedNow then
			setMessage("You joined up!")
			trigger.action.outSound(sndDING)
			numJoins = numJoins + 1			
		elseif timeOfLastBreak ~= nil then
			if (timeSinceStart - timeOfLastBreak) > maxRejoinTime then
				failMission("You rejoined too late. Mission failed.")
			end
		end
	end
	isJoined = isJoinedNow
end

--! Checks if the maximum number of breaks is exceeded, in which case it fails the mission.
function checkMaxBreaks()
	if numBreaks > maxBreaks then
		failMission("You broke formation too often. Mission failed.")
	end
end

--! Checks if the maximum distance exceeded, in which case it fails the mission.
function checkWaveOff()
	if curDist > waveOffDistance then
		failMission("You are way off. Mission failed.")
	end
end

--[[!
@brief Computes the score based on statistics over the distData and heightData arrays.

If the mean distance and mean height are within limits @p maxMnDist and @p maxMnHeight then
you score 1 point. If the standard deviation of the distance and height are within the limits @p maxStDevDist
and @p maxStdDevHeight then you score another point.

@returns score
]]
function computeScore()
	local mnDist = mean(distData)
	local stdDevDist = stdev(distData)
	local mnHeight = mean(heightData)
	local stDevHeight = stdev(heightData)
	
	local score
	
	-- Are the mean distance and height with in the required values?
	local closeEnough = mnDist < maxMnDist and math.abs(mnHeight) < maxMnHeight
	
	-- Are the standard deviation of the distance and height with in the required values?
	local steadyEnough = stdDevDist < maxStDevDist and stDevHeight < maxStdDevHeight
	if closeEnough then
		if steadyEnough then
			score = 2
		else
			score = 1
		end
	else
		score = 0
	end
	return score
end

--[[!
@brief Shows the current position, statistics over previous positions and feedback messages.

The current position text can be toggled on or off through the @p showPos boolean. The statistics text
is controlled by the @p showStats boolean. In-game the variables can be toggled through the F10 radio menu, if setupRadioMenu is called in the mission.
]]
function showText()
	posStr = ""
	if showPos then
		disStr = string.format("G = %.0f m, H = %.0f m", curDist, -curHeight)
		velStr = string.format("V = %.1f m/s", -curRadVel)
		if curRadVel < 0 then
		  velStr = velStr .. " (closing)"
		else
		  velStr = velStr .. " (moving away)"
		end
		posStr = disStr .. ", " .. velStr
	end
	
	statStr = ""
	if showStats and #distData > 0 then	
		statStr = string.format("\nmean D = %.1f m, stdev D = %.1f m, mean H = %.1f m, stdev H = %.1f m", mean(distData), stdev(distData), mean(heightData), stdev(heightData))
	end
	trigger.action.outText(posStr .. statStr .. "\n" .. msg, 1, true)
end

--[[!
@brief Evaluates the wingman's performance.

This function starts the evaluation if the mission time exceeds @p statStartTime and stops the evaluation
if the mission time exceeds @p statEndTime. After ending the stats the score is computed and a mission succes
or a mission fail is given.
]]
function checkEvaluation()
	local timeSinceStart = timer.getAbsTime() - timer.getTime0()
	if timeSinceStart > statStartTime and not statsRunning then
		if not isJoined then
			failMission("You did not join up in time. Mission failed.")
		else
			setMessage("Starting evaluation, keep it steady.")
			trigger.action.outSound(sndOK)
			statsRunning = true
		end
	elseif timeSinceStart > statEndTime and statsRunning then
		score = computeScore()
		if score == 2 then
			completeMission()
		elseif score==1 then
			failMission("You were not steady enough, mission failed ...")
		else
			failMission("You were not close enough, mission failed!")
		end
		statsRunning = false
	end
end

--! The main function, should be called continuously in the mission.
function main()
	if leader ~= nil and wingman ~= nil then
		updatePosition()
		showText()

		if not isFailed() and not isComplete() then
			checkJoined()
			checkMaxBreaks()
			checkWaveOff()
			checkEvaluation()
		end
	end
end

function toggleShowPosition()
	showPos = not showPos
end

function toggleShowStatistics()
	showStats = not showStats
end

function setupRadioMenu()
	missionCommands.addCommand("Toggle Show Position", nil, toggleShowPosition)
	missionCommands.addCommand("Toggle Show Statistics", nil, toggleShowStatistics)
end

function isFailed()
	return trigger.misc.getUserFlag(flagFail) == 1
end

function failMission(msg)
	trigger.action.setUserFlag(flagFail, 1)
	trigger.action.outSound(sndNOK)
	setMessage(msg)
end

function isComplete()
	return trigger.misc.getUserFlag(flagSucces) == 1
end

function completeMission()
	setMessage("Good job, mission succes!")
	trigger.action.setUserFlag(flagSucces,1)
	trigger.action.outSound(sndOK)
end

function setJoinedState(s)
	isJoined = s
end